#include "gm_driver.h"


//===================================================================================================================//
/************************************************????????CAN????????************************************************/
//===================================================================================================================//

/**
* @brief					GM6020???????????(??????)
	* @content 			HAL_CAN_AddTxMessage()
									
  * @param[out]		HAL_StatusTypeDef
  * @param[in]		CAN_HandleTypeDef
									int16_t Current * 4
  * @retval				HAL_StatusTypeDef
*/
HAL_StatusTypeDef GM6020Enable_4(CAN_HandleTypeDef * hcan,uint8_t mode, int16_t iq1, int16_t iq2 , int16_t iq3, int16_t iq4)
{
	static CAN_TxHeaderTypeDef	GM6020_DATA;
	uint8_t CAN_TX_DATA[8];

	GM6020_DATA.DLC = 0x08;
	GM6020_DATA.IDE = CAN_ID_STD;
	GM6020_DATA.RTR = CAN_RTR_DATA;
	if(mode==pre4_id)
	{
	GM6020_DATA.StdId = 0x1FF;
  }
	else
	{
		GM6020_DATA.StdId = 0x2FF;
	}
	
	//???	
	if(abs(iq1) > 25000u)
	{
		if(iq1>0)
			iq1=(short)25000;
		else
		  iq1=(short)-25000;		
	}
	
	else if(abs(iq2) > 25000u)
	{
		if(iq2>0)
			iq2=(short)25000;
		else
		  iq2=(short)-25000;		
	}
	else 	if(abs(iq3) > 25000u)
	{
		if(iq3>0)
			iq3=(short)25000;
		else
		  iq3=(short)-25000;		
	}
	else 	if(abs(iq4) > 25000u)
	{
		if(iq4>0)
			iq4=(short)25000;
		else
		  iq4=(short)-25000;		
	}
		
	CAN_TX_DATA[0] = iq1 >> 8;
	CAN_TX_DATA[1] = iq1;
	CAN_TX_DATA[2] = iq2 >> 8;
	CAN_TX_DATA[3] = iq2;
	CAN_TX_DATA[4] = iq3 >> 8;
	CAN_TX_DATA[5] = iq3;
	CAN_TX_DATA[6] = iq4 >> 8;
	CAN_TX_DATA[7] = iq4;

	if (HAL_CAN_AddTxMessage(hcan, &GM6020_DATA, CAN_TX_DATA, (uint32_t *)CAN_TX_MAILBOX0) == HAL_OK)		
		return HAL_OK;
	else 
		return HAL_ERROR;		
}


/**
* @brief					GM6020???????????(???????)
	* @content 			HAL_CAN_AddTxMessage()
									
  * @param[out]		HAL_StatusTypeDef
  * @param[in]		CAN_HandleTypeDef
									int16_t Current * 4
  * @retval				HAL_StatusTypeDef
*/
HAL_StatusTypeDef GM6020Enableiq_4(CAN_HandleTypeDef * hcan,uint8_t mode, int16_t iq1, int16_t iq2 , int16_t iq3, int16_t iq4)
{
	static CAN_TxHeaderTypeDef	GM6020_DATA;
	uint8_t CAN_TX_DATA[8];

	GM6020_DATA.DLC = 0x08;
	GM6020_DATA.IDE = CAN_ID_STD;
	GM6020_DATA.RTR = CAN_RTR_DATA;
	if(mode==pre4_id)
	{
	GM6020_DATA.StdId = 0x1FE;
  }
	else
	{
		GM6020_DATA.StdId = 0x2FE;
	}
//???	
	if(abs(iq1) > 16384u)
	{
		if(iq1>0)
			iq1=(short)16384;
		else
		  iq1=(short)-16384;		
	}
	
	else if(abs(iq2) > 16384u)
	{
		if(iq2>0)
			iq2=(short)16384;
		else
		  iq2=(short)-16384;		
	}
	else 	if(abs(iq3) > 16384u)
	{
		if(iq3>0)
			iq3=(short)16384;
		else
		  iq3=(short)-16384;		
	}
	else 	if(abs(iq4) > 16384u)
	{
		if(iq4>0)
			iq4=(short)16384;
		else
		  iq4=(short)-16384;		
	}
		
	
	CAN_TX_DATA[0] = iq1 >> 8;
	CAN_TX_DATA[1] = iq1;
	CAN_TX_DATA[2] = iq2 >> 8;
	CAN_TX_DATA[3] = iq2;
	CAN_TX_DATA[4] = iq3 >> 8;
	CAN_TX_DATA[5] = iq3;
	CAN_TX_DATA[6] = iq4 >> 8;
	CAN_TX_DATA[7] = iq4;

	if (HAL_CAN_AddTxMessage(hcan, &GM6020_DATA, CAN_TX_DATA, (uint32_t *)CAN_TX_MAILBOX0) == HAL_OK)		
		return HAL_OK;
	else 
		return HAL_ERROR;		
}



/**
  * @brief				????2006???????????
  * @param[out]		
  * @param[in]		hcan:?????CAN1
                  value:?????????????
  * @retval				
*/
HAL_StatusTypeDef GM2006Enable(CAN_HandleTypeDef * hcan,uint8_t mode, int16_t iq1, int16_t iq2 , int16_t iq3, int16_t iq4)
{
	static CAN_TxHeaderTypeDef  GM2006_DATA;
	uint8_t CAN_TX_DATA[8];

	GM2006_DATA.DLC = 0x08;
	GM2006_DATA.IDE = CAN_ID_STD;
	GM2006_DATA.RTR = CAN_RTR_DATA;
	
	if(mode==pre4_id)
	GM2006_DATA.StdId = 0x200;
	else
	GM2006_DATA.StdId = 0x1FF;
	
CAN_TX_DATA[0] = iq1 >> 8;
CAN_TX_DATA[1] = iq1;
CAN_TX_DATA[2] = iq2 >> 8;
CAN_TX_DATA[3] = iq2;
CAN_TX_DATA[4] = iq3 >> 8;
CAN_TX_DATA[5] = iq3;
CAN_TX_DATA[6] = iq4 >> 8;
CAN_TX_DATA[7] = iq4;

if (HAL_CAN_AddTxMessage(hcan, &GM2006_DATA, CAN_TX_DATA, (uint32_t *)CAN_TX_MAILBOX0) != HAL_OK)	
	{
     if(HAL_CAN_AddTxMessage(hcan, &GM2006_DATA, CAN_TX_DATA, (uint32_t *)CAN_TX_MAILBOX1) != HAL_OK)		
		 {
			 HAL_CAN_AddTxMessage(hcan, &GM2006_DATA, CAN_TX_DATA, (uint32_t *)CAN_TX_MAILBOX2);
		 }
		return HAL_OK;
	 }
else 
	return HAL_ERROR;		
}


/**
* @brief					M3508???????????
	* @content 			HAL_CAN_AddTxMessage()
									
  * @param[out]		HAL_StatusTypeDef
  * @param[in]		CAN_HandleTypeDef
					int16_t Current * 4
  * @retval			HAL_StatusTypeDef
*/
HAL_StatusTypeDef GM3508Enable(CAN_HandleTypeDef * hcan,uint8_t mode, int16_t iq1, int16_t iq2 , int16_t iq3, int16_t iq4)
{
	static CAN_TxHeaderTypeDef	GM3508_DATA;
	uint8_t CAN_TX_DATA[8];

	GM3508_DATA.DLC = 0x08;
	GM3508_DATA.IDE = CAN_ID_STD;
	GM3508_DATA.RTR = CAN_RTR_DATA;
	
	if(mode==pre4_id)
	GM3508_DATA.StdId = 0x200;
	else 
	GM3508_DATA.StdId = 0x1FF;		

CAN_TX_DATA[0] = iq1 >> 8;
CAN_TX_DATA[1] = iq1;
CAN_TX_DATA[2] = iq2 >> 8;
CAN_TX_DATA[3] = iq2;
CAN_TX_DATA[4] = iq3 >> 8;
CAN_TX_DATA[5] = iq3;
CAN_TX_DATA[6] = iq4 >> 8;
CAN_TX_DATA[7] = iq4;

if (HAL_CAN_AddTxMessage(hcan, &GM3508_DATA, CAN_TX_DATA, (uint32_t *)CAN_TX_MAILBOX0) == HAL_OK)		
	return HAL_OK;
else 
	return HAL_ERROR;		
}

